#include "vrat.h"

vRat::vRat(Stg::World *world, std::string& colour, ptree& config) : Stg::ModelPosition(world, NULL, colour)
{

	sun = NULL;
	draw_ir_beams = config.get<bool>("draw_ir_beams");

	float size_x = config.get<float>("size_x");
	float size_y = config.get<float>("size_y");
	float size_z = config.get<float>("size_z");

	float ir_range_min = config.get<float>("ir_range_min");
	float ir_range_max = config.get<float>("ir_range_max");

	unsigned camera_width = config.get<unsigned>("camera_width");
	unsigned camera_height = config.get<unsigned>("camera_height");
	float camera_elevation = config.get<float>("camera_elevation");
	float camera_pitch = config.get<float>("camera_pitch");
	float camera_fov_hori = config.get<float>("camera_fov_hori");
	float camera_fov_vert= config.get<float>("camera_fov_vert");

//	float camera_noise = config.get<float>("camera_noise");
//	float odometry_error = config.get<float>("odometry_error");

	bool collisions = config.get<bool>("collisions");

	this->colour = colour;

	// physical characteristics and appearance in Stage
	SetColor(colour);
	Stg::Geom g;
	g.size = Stg::Size(size_x, size_y, size_z);				// iRat size
	SetGeom(g);
	mass = 1.0;											// iRat mass

	SetBoundary(false);
	SetObstacleReturn(collisions);
	SetRangerReturn(1.0);
	SetGuiOutline(false);
	stack_children = false;

	Stg::Model *flapdragon = new Stg::Model(world, this, colour+"_flapdragon");
	flapdragon->SetRangerReturn(1.0);
	flapdragon->SetObstacleReturn(false);
	flapdragon->SetGeom(g);
	flapdragon->SetColor(colour);


	// ROS node setup
	nh = new ros::NodeHandle("/irat_" + colour);
	unsigned qsize = 1000;
	header.seq = 0;
	header.frame_id = "0";
	sub_stamp = ros::Time::now();

	// ROS publishers
	p_cam = new ros::Publisher(nh->advertise<sensor_msgs::CompressedImage>("camera/images/compressed", qsize));
	p_cam_raw = new ros::Publisher(nh->advertise<sensor_msgs::Image>("camera/images/raw", qsize));
	p_odom = new ros::Publisher(nh->advertise<irat_msgs::IRatVelocity>("serial/odometry", qsize));
	p_ranger = new ros::Publisher(nh->advertise<irat_msgs::IRatRangers>("serial/rangers", qsize));
	p_pose = new ros::Publisher(nh->advertise<geometry_msgs::Pose2D>("pose", qsize));
	p_power = new ros::Publisher(nh->advertise<irat_msgs::IRatPower>("serial/power", qsize));

	// ROS subscribers
	s_dist = new ros::Subscriber(nh->subscribe<irat_msgs::IRatDistance>("serial/cmddist", qsize, &vRat::callback_dist, this));
	s_vel = new ros::Subscriber(nh->subscribe<irat_msgs::IRatVelocity>("serial/cmdvel", qsize, &vRat::callback_vel, this));

	// Stage IR sensors
	stg_ranger = new Stg::ModelRanger(world, this, colour + "_ir");
	stg_ranger->SetColor(Stg::Color("yellow"));

	Stg::ModelRanger::Sensor ir;
	ir.range = Stg::Bounds(ir_range_min, ir_range_max);		// set IR sensor characteristics here
	stg_ranger->GetSensorsMutable().resize(3, ir);
	stg_ranger->GetSensorsMutable()[0].pose.a = -M_PI_4;	// set IR sensor poses here
	stg_ranger->GetSensorsMutable()[1].pose.a = 0;
	stg_ranger->GetSensorsMutable()[2].pose.a = M_PI_4;
	for (unsigned i = 0; i < stg_ranger->GetSensorsMutable().size(); i++)
		stg_ranger->GetSensorsMutable()[i].Update(stg_ranger);

	// ROS IR sensors
	irat_msgs::IRatRanger ros_range;
	ros_range.field_of_view = 0;
	ros_range.min_range = ir_range_min;
	ros_range.max_range = ir_range_max;
	ros_ranger.rangers.resize(3, ros_range);

	// Stage Camera
	stg_cam = new RatCamera(world, this, camera_width, camera_height, colour + "_camera");
	stg_cam->SetPose(Stg::Pose(0, 0, camera_elevation, 0));
	stg_cam->setPitch(camera_pitch);
	stg_cam->setFOV(camera_fov_hori, camera_fov_vert);

	// ROS Camera
	ros_img.format = "jpeg";
	ros_img_raw.width = stg_cam->getWidth();
	ros_img_raw.height = stg_cam->getHeight();
	ros_img_raw.step = 3*stg_cam->getWidth();
	ros_img_raw.encoding = "bgr8";
	ros_img_raw.data.resize(ros_img_raw.height*ros_img_raw.step);

	Subscribe(); Startup();

	Stg::Pose p(0, 0, 0.15, -M_PI_2);
	ply = new ModelPLY(p, "models/horse.ply");
	ply->setScale(2);

	stall_av = 0;

}

vRat::~vRat()
{
	// clean up ROS
	delete p_cam;
	delete p_cam_raw;
	delete p_odom;
	delete p_pose;
	delete p_ranger;
	delete p_power;
	delete s_dist;
	delete s_vel;

	// clean up Stage
	delete stg_ranger;
	delete stg_cam;
}

void vRat::Update()
{

	double theta;
	double x = stg_ranger->GetSensors()[0].ranges[0], y = stg_ranger->GetSensors()[1].ranges[0], z = stg_ranger->GetSensors()[2].ranges[0];
	theta = (0.2 - x)*10.0; // scott
	theta=(x<.25&&z<.25)?(z-x)*15:(x<.25)?(7/x):(z<.25)?(-7/z):(y<.6)?30:(.8*((float)rand()/RAND_MAX-.3)); // paul
	theta = ((y<.6 || z<.2 || x<.2) && z < x)?-.80:.80; // phil
	theta=10*(z-x)/(y?y:1); // sam
//	SetSpeed(0.2, 0, theta);

	if (ros::Time::now() > sub_stamp + ros::Duration(0.5))
		SetSpeed(0, 0, 0);

	Stg::ModelPosition::Update();
}

void vRat::Move()
{
	//Update();
	Stg::ModelPosition::Move();

	if (stall)
	{
		Stg::Velocity v = GetVelocity();
		if (!v.IsZero())
		{
			v.x = -v.x;
			v.y = -v.y;
			SetVelocity(v);
			stall = false;
			Stg::ModelPosition::Move();
		}
	}

	// setup header
	header.stamp = ros::Time::now();
	header.seq++;

	// publish absolute pose
	geometry_msgs::Pose2D ros_pose;
	Stg::Pose stg_pose = GetGlobalPose();
	ros_pose.x = stg_pose.x;
	ros_pose.y = stg_pose.y;
	ros_pose.theta = stg_pose.a;
	p_pose->publish(ros_pose);

	// publish odometry
	irat_msgs::IRatVelocity ros_vel;
	ros_vel.angle = GetVelocity().a;
	ros_vel.magnitude = GetVelocity().x;
	ros_vel.header = header;
	p_odom->publish(ros_vel);

	// publish rangers
	for (unsigned i = 0; i < stg_ranger->GetSensorsMutable().size(); i++)
	{
		stg_ranger->GetSensorsMutable()[i].Update(stg_ranger);
		ros_ranger.rangers[i].range = stg_ranger->GetSensors()[i].ranges[0];
	}
	ros_ranger.header = header;
	p_ranger->publish(ros_ranger);

	// publish compressed image
	stg_cam->Update();
	stg_cam->SetupCamera();

	if (sun)
	{
		glEnable(GL_LIGHTING);
		glEnable(GL_LIGHT0);

		GLfloat ambient[] = {.2, .2, .2, 1.0};

		if (sun->z > 0.0)
		{
			glLightfv(GL_LIGHT0, GL_POSITION, &(sun->x));
			GLfloat specular[] = {.9, .9, .9, 1.0};
			GLfloat diffuse[] = {.8, .8, .8, 1.0};
			float atten_lin = 1.0;
			float atten_const = 0.0;

			glLightfv(GL_LIGHT0, GL_QUADRATIC_ATTENUATION, &atten_lin);
			glLightfv(GL_LIGHT0, GL_CONSTANT_ATTENUATION, &atten_const);

			glLightfv(GL_LIGHT0, GL_AMBIENT, ambient);
			glLightfv(GL_LIGHT0, GL_DIFFUSE, diffuse);
			glLightfv(GL_LIGHT0, GL_SPECULAR, specular);
		}
		else
		{
			GLfloat empty[] = {0, 0, 0, 1.0};
			glLightfv(GL_LIGHT0, GL_AMBIENT, ambient);
			glLightfv(GL_LIGHT0, GL_DIFFUSE, empty);
			glLightfv(GL_LIGHT0, GL_SPECULAR, empty);
		}
	}

	stg_cam->GetFrame();

	if (sun)
	{
		glDisable(GL_LIGHTING);
		glDisable(GL_LIGHT0);
	}

	if (sun && sun->z > 0.0)
	{
		glPointSize(20.0);
		glBegin(GL_POINTS);
		glColor4f(1.0, 0, 0, 1.0);
		glVertex3f(sun->x, sun->y, sun->z);
		glEnd();
	}

	unsigned w = stg_cam->getWidth();
	unsigned h = stg_cam->getHeight();
	cv::Mat uncompressed(h, w, CV_8UC3);
	memcpy(uncompressed.ptr(0), stg_cam->FrameColor(), w*h*3);

/*
	// ir sensor test
	uchar r0 = 255*stg_ranger->GetSensors()[0].ranges[0]/(stg_ranger->GetSensors()[0].range.max - stg_ranger->GetSensors()[0].range.min);
	uchar r1 = 255*stg_ranger->GetSensors()[1].ranges[0]/(stg_ranger->GetSensors()[1].range.max - stg_ranger->GetSensors()[1].range.min);
	uchar r2 = 255*stg_ranger->GetSensors()[2].ranges[0]/(stg_ranger->GetSensors()[2].range.max - stg_ranger->GetSensors()[2].range.min);
	for (unsigned i = 0; i < w; i++)
	{
		uchar v;
		if (i < w*1.0/3) v = r0;
		else if (i < w*2.0/3) v = r1;
		else v = r2;
		for (unsigned j = 0; j < h; j++)
		{
			uchar *ptr = uncompressed.ptr(0)+3*(w*j + i);
			ptr[0] = v;
			ptr[1] = v;
			ptr[2] = v;
			ptr[3] = 255;
		}
	}
*/

	imencode(".jpg", uncompressed, ros_img.data);
	ros_img.header = header;
	p_cam->publish(ros_img);

	// publish image
	memcpy(&(*ros_img_raw.data.begin()), uncompressed.ptr(0), w*h*3);
	ros_img_raw.header = header;
	p_cam_raw->publish(ros_img_raw);

	// publish power
	irat_msgs::IRatPower ros_power;
	ros_power.voltage = 12.0;
	ros_power.header = header;
	p_power->publish(ros_power);
}

void vRat::DrawBlocks()
{

	glPushMatrix();
	ModelPosition::DrawBlocks();
	glPopMatrix();

	glEnable(GL_COLOR_MATERIAL);
	glShadeModel(GL_SMOOTH);
	glPolygonMode(GL_FRONT, GL_FILL);

	/*
	glPushMatrix();
	Stg::Gl::pose_shift(ply->GetPose());
	ply->DrawBlocks();
	glPopMatrix();
	*/

	if (draw_ir_beams)
	{
		glDisable(GL_LIGHTING);
		glDisable(GL_LIGHT0);
		// draw IR sensor beams (because Stage's isn't very good)
		glColor3f(1, 0, 0);
		for (unsigned i = 0; i < stg_ranger->GetSensors().size(); i++)
		{
			const Stg::ModelRanger::Sensor *ir = &stg_ranger->GetSensors()[i];
			glPushMatrix();
			Stg::Gl::pose_shift(ir->pose);
			glBegin(GL_LINES);
			glVertex3f(0, 0, 0);
			glVertex3f(ir->ranges[0], 0, 0);
			glEnd();
			glPopMatrix();
		}
		glEnable(GL_LIGHT0);
		glEnable(GL_LIGHTING);
	}
}

void vRat::callback_vel(const irat_msgs::IRatVelocityConstPtr& msg)
{
	sub_stamp = ros::Time::now();
	SetSpeed(msg->magnitude, 0, msg->angle);
}

void vRat::callback_dist(const irat_msgs::IRatDistanceConstPtr& msg)
{
	//GoTo(msg->magnitude*cos(msg->angle), msg->magnitude*sin(msg->angle), 0);
}
